import threading
import rclpy
from rclpy.node import Node
from robot_interface.msg import VisionInfo,VisionCtrl
from onvif import ONVIFCamera
from flask import Flask, Response,request,jsonify
from .vision_proc import *
import cv2
import time
import math
import logging
class OnvifCameraNode(Node):
    '''
        机器人主摄像头驱动程序
    '''
    #   2K摄像头电子变焦参数
    #ZOOM_PARAM=

    def __init__(self):
        super().__init__('cam_driver')
        
        #   状态缓存
        self.state_buffer=VisionInfo()
        self.vision_frame_raw=None
        self.digital_zoom=1.0
        self.zoom_params=[0,0,2560,1440]
        self.resolution=[0,0]

        #   申明参数
        log = logging.getLogger('werkzeug')
        log.setLevel(logging.ERROR)
        self.DeclareParams()
        #   申明接口
        self.DeclareInterfaces()
        #   开启线程
        self.StartThreads()
    
    def DeclareParams(self):
        '''
            声明参数
        '''
        #   ONVIF参数
        self.declare_parameter('cam_ip','192.168.1.68')
        self.declare_parameter('cam_account','admin')
        self.declare_parameter('zoom_max',25.0)
        self.declare_parameter('angle_min',2.9*math.pi/180.0)
        self.declare_parameter('cam_passwd','a1234567')
        self.declare_parameter('cam_state_publish_freq',30.0)
        self.declare_parameter('safe_transfer',True)

        self.cam_ip=self.get_parameter('cam_ip').get_parameter_value().string_value
        self.cam_account=self.get_parameter('cam_account').get_parameter_value().string_value
        self.cam_passwd=self.get_parameter('cam_passwd').get_parameter_value().string_value
        self.zoom_max=self.get_parameter('zoom_max').get_parameter_value().double_value
        self.angle_min=self.get_parameter('angle_min').get_parameter_value().double_value
        self.pub_freq=self.get_parameter('cam_state_publish_freq').get_parameter_value().double_value
        self.safe_transfer=self.get_parameter("safe_transfer").get_parameter_value().bool_value
        

        #   视频参数
        self.declare_parameter('/mjpeg_port',8888)
        self.declare_parameter('/stream_quality',60)
        self.declare_parameter('/gray_mode', False)

        self.mjpeg_port=self.get_parameter('/mjpeg_port').get_parameter_value().integer_value   #  视频流端口
        self.stream_quality=self.get_parameter('/stream_quality').get_parameter_value().integer_value    #  视频流质量
        self.gray_mode=self.get_parameter('/gray_mode').get_parameter_value().bool_value

    def DeclareInterfaces(self):
        '''
            声明接口
        '''
        #   ONVIF对接摄像头
        self.cam = ONVIFCamera(self.cam_ip, 80, self.cam_account, self.cam_passwd)
        self.media = self.cam.create_media_service()
        self.ptz = self.cam.create_ptz_service()
        self.image = self.cam.create_imaging_service()

        # 获取配置
        profiles = self.media.GetProfiles()
        self.profile_token = profiles[0].token
        self.ptz.GotoHomePosition({'ProfileToken': self.profile_token})
        # 获取视频流地址
        stream_setup = {
            'Stream': 'RTP-Unicast',
            'Transport': {'Protocol': 'RTSP'}
        }
        media_profile = self.media.GetProfile({'ProfileToken': self.profile_token})
        video_url = self.media.GetStreamUri({'StreamSetup': stream_setup,'ProfileToken': media_profile.token}).Uri
        if self.safe_transfer:
            header,body=video_url.split('//')
            video_url=header+'//'+self.cam_account+":"+self.cam_passwd+"@"+body
        self.state_buffer.url=video_url


        # #HTTP服务器
        self.server=Flask(__name__)
        self.server.add_url_rule('/stream',view_func=self.VideoFeed,methods=['POST','GET'])     #视频流
        self.server.add_url_rule('/raw_image',view_func=self.ImageFeed,methods=['POST','GET'])  #高清2K图像
        self.server.add_url_rule('/ptz_ctrl',view_func=self.PTZCtrl,methods=['POST'])           #云台控制
        #self.server.add_url_rule('/track',view_func=self.TrackCtrl,methods=['POST'])     #跟踪目标
        self.server.add_url_rule('/status',view_func=self.Status,methods=['GET'])        #返回当前状态
    
    def StartThreads(self):
        '''
            开启独立线程
            1. 更新视频采集信息
            2. 网页服务器
            3. 无人机识别跟踪算法
        '''
        #   开启线程
        threads_pool=[threading.Thread(target=self.__UpdateVision),
                      threading.Thread(target=self.__StartVisionServer)]
        for thread in threads_pool:
            thread.setDaemon(True)
            thread.start()

    '''''''''''''''''''''''''''''''''
        线程函数
    '''
    def __UpdateVision(self):
        '''
            更新视频数据帧
        '''
        #   等待摄像头节点开启
        cap=cv2.VideoCapture(self.state_buffer.url)
        while self.resolution[0]==0:
            time.sleep(0.02)
            success,self.vision_frame_raw=cap.read()
            self.resolution=self.vision_frame_raw.shape[:2]
        #   循环读取摄像头数据
        while True:
            success,self.vision_frame_raw=cap.read()
            #   获取原始图像分辨率
            self.resolution=self.vision_frame_raw.shape[:2]
            if self.digital_zoom>1.01:
                self.vision_frame_raw=self.vision_frame_raw[self.zoom_params[0]:self.zoom_params[1],self.zoom_params[2]:self.zoom_params[3]]
            if not success: self.get_logger().info('摄像头数据流读取错误')
            time.sleep(0.02)

    def __StartVisionServer(self):
        '''
            开启视觉服务
        '''
        self.server.run(host='0.0.0.0',port=self.mjpeg_port)
    '''''''''''''''''''''''''''''''''
        Flask回调函数
    '''
    def GenFrames(self):
        '''
            返回视频数据流
        '''
        while True:
            # 50Hz
            time.sleep(0.02)
            if self.vision_frame_raw is None: continue
            '''
                此处添加视频处理的代码
            '''
            frame_ret=cv2.resize(self.vision_frame_raw,(960,540))
            if self.gray_mode:
                frame_ret=cv2.cvtColor(frame_ret,cv2.COLOR_BGR2GRAY)
            ret, buffer = cv2.imencode('.jpg',frame_ret,[int(cv2.IMWRITE_JPEG_QUALITY), self.stream_quality])
            buffer=buffer.tobytes()
            yield (b'--frame\r\n'
                   b'Content-Type: image/jpeg\r\n\r\n' + buffer + b'\r\n')
            
    def ImageFeed(self):
        '''
        返回原始图像
        '''
        ret, buffer = cv2.imencode('.jpg', self.vision_frame_raw, [int(cv2.IMWRITE_JPEG_QUALITY), 100])
        buffer=buffer.tobytes()
        return Response(buffer, mimetype='image/jpeg')
    
    def VideoFeed(self):
        '''
        返回视频数据
        '''
        return Response(self.GenFrames(),mimetype='multipart/x-mixed-replace; boundary=frame')
    
    def Move(self,vx,vy,vzoom):
        '''
            移动摄像头（速度指令）
        '''
        velocity = {'PanTilt': {'x': vx, 'y': vy},'Zoom': {'x': vzoom}} 
        self.ptz.ContinuousMove({'ProfileToken': self.profile_token,'Velocity': velocity})

    def FixTo(self,x,y,zoom):
        '''
            固定指向
        '''
        threading.Thread(target=self.__FixTo,args=(x,y,zoom)).start()
        
    
    def __FixTo(self,x,y,z):
        '''独立线程'''
        req = self.ptz.create_type('AbsoluteMove')
        req.ProfileToken = self.profile_token
        req.Position = {'PanTilt': {'x': -Rad2ONVIF(x), 'y': Rad2ONVIF(y)},'Zoom': {'x': 0.05}} 
        self.ptz.AbsoluteMove(req)
        time.sleep(1.5)
        self.ZoomTo(z)

    
    def ZoomTo(self,zoom):
        '''
            调整焦距
        '''
        if zoom <1: zoom=1
        if zoom>20: zoom=20
        cont_zoom=self.ptz.GetStatus({'ProfileToken': self.profile_token}).Position.Zoom.x   #当前缩放
        target_zoom=zoom/self.zoom_max
        if target_zoom>cont_zoom:
            self.Move(0.0,0.0,0.1)
            while(self.ptz.GetStatus({'ProfileToken': self.profile_token}).Position.Zoom.x<target_zoom): 
                time.sleep(0.01)
        else :
            self.Move(0.0,0.0,-0.1)
            while(self.ptz.GetStatus({'ProfileToken': self.profile_token}).Position.Zoom.x>target_zoom): 
                time.sleep(0.01)
        self.Move(0.0,0.0,0.0)
        return self.ptz.GetStatus({'ProfileToken': self.profile_token}).Position.Zoom.x

    def PTZCtrl(self):
        '''
            摄像头控制接口
        '''
        data={}
        #try:
        data = request.get_json()
        '''
            PTZ直接控制
        '''
        if data['mode']=='ptz_move': 
            '''
                PTZ-移动模式
            '''
            self.Move(data['vx'],data['vy'],data['vzoom'])
        elif data['mode']=='ptz_fix': 
            '''
                PTZ-位置模式
            '''
            self.FixTo(data['x'],data['y'],data['zoom'])
        elif data['mode']=='ptz_zoom': 
            '''
                仅缩放模式
            '''
            self.ZoomTo(data['zoom'])
        elif data['mode']=='digital_zoom':
            '''
                电子缩放
            '''
            self.digital_zoom=data['zoom']
            if 1<=self.digital_zoom<=2.5:
                tar_height=int(self.resolution[0]/self.digital_zoom)
                tar_width=int(self.resolution[1]/self.digital_zoom)
                x1=int((self.resolution[1]-tar_width)/2)
                x2=x1+tar_width
                y1=int((self.resolution[0]-tar_height)/2)
                y2=y1+tar_height
                self.zoom_params=[y1,y2,x1,x2]


        elif data['mode']=='xyz_coord':
            '''
            位置坐标控制
            '''
            pan,tilt,zoom=XYZ2ONVIFParams(data['x'],data['y'],data['z'])
            self.FixTo(pan,tilt,zoom)
        
        elif data['mode']=='bds_coord':
            '''
            位置坐标控制
            '''
            x,y,z=XYZ2ONVIFParams(data['latitute'],data['longitute'],data['height'])
            self.FixTo(x,y,z)

        # except Exception as e:
        #     return jsonify({"error": str(e)}), 500
        return jsonify({"status": "ok"}), 200
    
        
    def Status(self):
        '''
            持续发布当前摄像头状态
        '''
        
        bundle=self.ptz.GetStatus({'ProfileToken': self.profile_token})
        zoom=(bundle.Position.Zoom.x)*self.zoom_max
        if zoom<1: zoom=1
        pan=-ONVIF2Rad(bundle.Position.PanTilt.x)
        tilt=ONVIF2Rad(bundle.Position.PanTilt.y)
        return jsonify({"pan":pan,"tilt":tilt,"zoom":zoom,"fov":self.angle_min*self.zoom_max/zoom,"resolution":self.zoom_params,"digital_zoom": self.digital_zoom}), 200


def main(args=None):
    rclpy.init(args=args)
    node = OnvifCameraNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
